% Implementation of Super Twisting Control
% ym is the measurable output and I set the output to 0 after 5 seconds
% x1 will diconverge to infinity

params=struct();
params.time=0:0.01:10;
params.initial=[2 -1];
params.delta=rand(size(params.time));
params.k=[1 2];
[~,s0]=model(0,params.initial,params);

[t,y]=ode45(@(t,x)model(t,x,params),params.time,params.initial);
s=repmat(s0,size(t,1),1);
for it=1:length(t)
    [~,si]=model(t(it),y(it,:)',params);
    s(it,:)=si;
end


figure;tiledlayout(3,1)
nexttile;hold on;grid on;
plot(t,y(:,1));
plot(t,y(:,2));
legend("x1","x2");
nexttile;hold on;grid on;
plot(t,vertcat(s.dxdt1))
plot(t,vertcat(s.dxdt2))
legend("$\dot{x}_1$","$\dot{x}_2$","Interpreter","latex");
nexttile;hold on;grid on;
plot(t,vertcat(s.delta));
legend("delta");


function [dydt,s]=model(t,y,p)
    x1=y(1);x2=y(2);
    s.delta=interp1(p.time,p.delta,t,"cubic");
    ym=x1;
    if t>5
        ym=0;
    end
    s.dxdt1=-p.k(1)*abs(ym)*sign(x1)+x2;
    s.dxdt2=-p.k(2)*sign(ym)+s.delta;
    dydt=[s.dxdt1;s.dxdt2];
end